/*
 * 文件名：bsp_motor.cpp
 * 功能描述：步进电机的初始化及驱动控制
 */
#include <Arduino.h>
#include "bsp_motor.h"
#include "power_management.h"

// 电机定义,设置电机的脉冲和方向
AccelStepper Xaxis(1, STEP_PIN, DIR_PIN);   // step, direction
AccelStepper Yaxis(1, STEP_PIN2, DIR_PIN2); // step, direction
//多电机控制器
MultiStepper steppersControl;               // Create instance of MultiStepper

// // Scara宏定义
// const float x_length_g = 0.05;
// const float y_length_g = 0.05;
// float radius_g = x_length_g+y_length_g;

// unsigned int x_arm_ratio = 1;
// unsigned int y_arm_ratio = 1;
// unsigned int x_microstep = 8;
// unsigned int y_microstep = 8;
// unsigned int x_circle = (200 * x_microstep * x_arm_ratio);
// unsigned int y_circle = (200 * y_microstep * y_arm_ratio);
unsigned int x_circle = (HBOT_STEP_DIS * X_RATIO);
unsigned int y_circle = (HBOT_STEP_DIS * Y_RATIO);

float gotoposition[2];
long int_gotoposition[2];
float motor1_max_speed_g = 0;
float motor2_max_speed_g = 0;
float motor1_speed_g = 0;
float motor2_speed_g = 0;
float motor1_acc_g = 0;
float motor2_acc_g = 0;
uint8_t x_current_dir_g;
uint8_t y_current_dir_g;
//_______________________________________________
// 函数：x_run_degree
// 功能：根据角度计算弧长
// 参数：degree：角度
// 返回值：弧长
int x_run_degree(float degree)
{
    // 定义变量ret，用于存储弧长
    float ret = 0;
    // 计算弧长，x_circle为圆的半径，degree为角度
    ret = x_circle * (degree / 360.0);

    // 返回弧长
    return int(ret);
}

// 函数：y_run_degree
// 功能：根据角度计算y轴的移动距离
// 参数：degree：角度
// 返回值：y轴的移动距离
int y_run_degree(float degree)
{
    // 定义变量ret，用于存储计算结果
    float ret = 0;
    // 计算y轴的移动距离，公式为：y轴的移动距离 = 圆的周长 * 角度 / 360
    ret = y_circle * (degree / 360.0);

    // 返回计算结果，类型为整数
    return int(ret);
}

float recordy = 0;
float recordx = 0;

// 将弧度转换为实际运行距离
float x_run_radian(float radian)
{
    float ret = 0;

    // 将弧度转换为实际运行距离
    ret = x_circle * (radian / (2 * PI));
    // 将实际运行距离累加到recordx中
    recordx = recordx + ret;
    // 打印recordx的值，保留6位小数
    // Serial.print(recordx, 6);
    return ret;
}

// 根据弧度计算y轴的运行距离
float y_run_radian(float radian)
{
    float ret = 0;

    // 计算弧度对应的y轴距离
    ret = y_circle * (radian / (2 * PI));
    // 更新记录的y轴距离
    recordy = recordy + ret;
    // Serial.print(" ");
    // Serial.println((recordx + recordy), 6);
    return ret;
}

// 将弧度转换为角度
float radian_to_degree(float radian)
{
    float ret = 0;

    // 将弧度转换为角度，公式为：角度 = 360 * (弧度 / (2 * PI))
    ret = 360.0 * (radian / (2 * PI));
    return ret;
}

// 将步数转换为角度
float steps_to_degree(int steps, int one_circle)
{
    float ret = 0;

    // 将步数转换为角度
    ret = 360.0 * steps / (one_circle * 1.0);

    return ret;
}
// 将步数转换为弧度
float steps_to_radian(int steps, int one_circle)
{
    float ret = 0;

    // 计算步数对应的弧度
    ret = 2 * PI * steps / (one_circle * 1.0);

    return ret;
}

/**
 * @brief 步进电机系统初始化
 *
 * 配置电机控制引脚方向/使能信号，设置运动参数：
 * - 初始化X/Y轴电机引脚模式
 * - 设置最大速度/加速度
 * - 归零电机位置计数器
 * - 添加电机到多步进控制器
 */
void motor_init(void)
{

    pinMode(EN_PIN, OUTPUT);
    pinMode(STEP_PIN, OUTPUT);
    pinMode(DIR_PIN, OUTPUT);

    pinMode(EN_PIN2, OUTPUT);
    pinMode(STEP_PIN2, OUTPUT);
    pinMode(DIR_PIN2, OUTPUT);
    //配置默认旋转方向
    x_current_dir_g = MOTOR_X_DEFAULT_DIR;
    y_current_dir_g = MOTOR_Y_DEFAULT_DIR;
    digitalWrite(DIR_PIN, x_current_dir_g);
    digitalWrite(DIR_PIN2, y_current_dir_g);
    //_______________________________________________
    //设置速度
    Xaxis.setMaxSpeed(speed_g);
    Yaxis.setMaxSpeed(speed_g);
    //设置加速度
    Xaxis.setAcceleration(speed_g / sqrt(2));
    Yaxis.setAcceleration(speed_g / sqrt(2));
    //设置默认位置
    Xaxis.setCurrentPosition(0);
    Yaxis.setCurrentPosition(0);
    //多电机控制器配置
    steppersControl.addStepper(Xaxis);
    steppersControl.addStepper(Yaxis);

    //MOTOR_DISABLE;
    MOTOR_ENABLE;
    motor_enable = true;
}